# AprilTags Example
#
# This example shows the power of the OpenMV Cam to detect April Tags
# on the OpenMV Cam M7. The M4 versions cannot detect April Tags.

import sensor, image, time, math
from pid import PID
from pyb import Servo

#s1 = Servo(1) # P7
#s2 = Servo(2) # P8

pan_servo=Servo(1)
tilt_servo=Servo(2)

pan_servo.calibration(500,2500,500)
tilt_servo.calibration(500,2500,500)

pan_pid = PID(p=0.27, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID
tilt_pid = PID(p=0.25, i=0, imax=90) #脱机运行或者禁用图像传输，使用这个PID

sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) # we run out of memory if the resolution is much bigger...
sensor.skip_frames(30)
sensor.set_auto_gain(False)  # must turn this off to prevent image washout...
sensor.set_auto_whitebal(False)  # must turn this off to prevent image washout...
clock = time.clock()

# 注意！与find_qrcodes不同，find_apriltags 不需要软件矫正畸变就可以工作。

# 注意，输出的姿态的单位是弧度，可以转换成角度，但是位置的单位是和你的大小有关，需要等比例换算

# f_x 是x的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/3.984*656，这个值是用毫米为单位的焦距除以x方向的感光元件的长度，乘以x方向的感光元件的像素（OV7725）
# f_y 是y的像素为单位的焦距。对于标准的OpenMV，应该等于2.8/2.952*488，这个值是用毫米为单位的焦距除以y方向的感光元件的长度，乘以y方向的感光元件的像素（OV7725）

# c_x 是图像的x中心位置
# c_y 是图像的y中心位置
img_framesize_x=160
img_framesize_y=120

f_x = (2.8 / 3.984) * img_framesize_x # 默认值
f_y = (2.8 / 2.952) * img_framesize_y # 默认值
c_x = img_framesize_x * 0.5 # 默认值(image.w * 0.5)
c_y = img_framesize_y * 0.5 # 默认值(image.h * 0.5)

def degrees(radians):
    return (180 * radians) / math.pi

def Pid_control(img1):
    pan_error = img1.cx()-img.width()/2
    tilt_error = img1.cy()-img.height()/2
    pan_output=pan_pid.get_pid(pan_error,1)/2
    tilt_output=tilt_pid.get_pid(tilt_error,1)
    pan_servo.angle(pan_servo.angle()+pan_output)
    tilt_servo.angle(tilt_servo.angle()-tilt_output)
    speed =(pan_servo.angle()+pan_output,tilt_servo.angle()-tilt_output)
    return speed

def AprilTag(img):
    for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # 默认为TAG36H11
        img.draw_rectangle(tag.rect(), color = (255, 0, 0))
        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
        speed = Pid_control(tag)
        print_args = (tag.id(), tag.x_translation(), tag.y_translation(), tag.z_translation(), \
            degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()),\
            speed[0],speed[1])
        return print_args
        # 位置的单位是未知的，旋转的单位是角度
        # PID控制

def AprilTag_v01(img):
    for tag in img.find_apriltags(fx=f_x, fy=f_y, cx=c_x, cy=c_y): # 默认为TAG36H11
        img.draw_rectangle(tag.rect(), color = (255, 0, 0))
        img.draw_cross(tag.cx(), tag.cy(), color = (0, 255, 0))
        print_args = (tag.id(), tag.x_translation(), tag.y_translation(), tag.z_translation(), \
            degrees(tag.x_rotation()), degrees(tag.y_rotation()), degrees(tag.z_rotation()))
        return print_args

Target_count=0
tilt_servo.pulse_width(1750)
while(True):
    clock.tick()
    img = sensor.snapshot()
    #旋转寻找目标
    Apri=AprilTag(img)
    if  Apri== None:
        Target_count+=1
    else:
        Target_count=0
        Find_count=3

    if  Target_count >= 5:
        Target_count=0
        for j in range(1750,2250,250):
            tilt_servo.pulse_width(j)
            if j == 1750 :
                for i in range(0,2501,500):
                    img1 = sensor.snapshot()
                    if  AprilTag_v01(img1) != None:
                        break
                    pan_servo.pulse_width(i)
                    time.sleep_ms(500)
            else:
                for i in range(2501,1,-500):
                    img2 = sensor.snapshot()
                    if  AprilTag_v01(img2) != None:
                        break
                    pan_servo.pulse_width(i)
                    time.sleep_ms(500)
        #for i in range(300,1800,50):
            #img2 = sensor.snapshot()
            #if  AprilTag_v01(img2) != None:
                  #break
            #tilt_servo.pulse_width(200+i)
            #time.sleep_ms(100)
    else:
        print(Apri)
        print(clock.fps())


